
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_scurve.h
  * @author     baiyang
  * @date       2022-9-5
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*
 * SCurves calculate paths between waypoints (including the corners) using specified speed, acceleration and jerk limits
 *
 * How to use:
 *  1. create three SCurve objects called something like "prev_leg", "this_leg" and "next_leg"
 *  2. call this_leg.calculate_track() to calculate the path from the origin to the destination for the given speed, accel and jerk limits
 *  3. if the vehicle will fly past the destination to another "next destination":
 *        a) call next_leg.calculate_track() with the appropriate arguments
 *        b) set a "fast_waypoint" boolean to true.  this will be passed into "advance_target_along_track()" in the next step
 *     if there is no "next destination"
 *        a) call next_leg.init()
 *        b) set the "fast_waypoint" boolean to false
 *  4. call this_leg.advance_target_along_track() with a small "dt" value and retrieve the resulting target position, velocity and acceleration
 *     Note: the target_pos should be set to the segments's earth frame origin before this function is called
 *  5. pass the target position, velocity and acceleration into the position controller
 *  6. repeat steps 4 and 5 until finished() returns true
 *  7. promote the legs:
 *        a) set prev_leg = this_leg
 *        b) set this_leg = next_leg
 *        c) jump back to step 3
 *
 * Other features:
 *  1. set_speed_max() allows changing the max speeds mid path.  The path will be recalculated
 *  2. set_origin_speed_max() and set_destination_speed_max() allows setting the speed along the path at the beginning and end of the leg
 *     this is used to smoothly integrate with spline segments
 *
 * This library works with any units (meters, cm, etc) as long as they are used consistently.
 *    e.g. if origin and destination are meters, speeds should be in m/s, accel in m/s/s, etc.
 *
 * Terminology:
 *    position: a point in space
 *    velocity: rate of change of position.  aka speed
 *    acceleration: rate of change of speed
 *    jerk: rate of change of acceleration
 *    snap: rate of change of jerk
 *    jerk time: the time (in seconds) for jerk to increase from zero to its maximum value
 *    track: 3D path that the vehicle will follow
 *    path: position, velocity, accel and jerk kinematic profile that this library generates
 */

/*----------------------------------include-----------------------------------*/
#include "gp_mathlib.h"
/*-----------------------------------macro------------------------------------*/
#define segments_max 23
/*----------------------------------typedef-----------------------------------*/
// segment types
enum SegmentType {
    SCURVE_CONSTANT_JERK,
    SCURVE_POSITIVE_JERK,
    SCURVE_NEGATIVE_JERK
};

typedef struct scurve* scurve_t;

/** @ 
  * @brief  
  */
struct scurve {
    // members
    float snap_max;     // maximum snap magnitude
    float jerk_max;     // maximum jerk magnitude
    float accel_max;    // maximum acceleration magnitude
    float vel_max;      // maximum velocity magnitude
    float time;         // time that defines position on the path
    float position_sq;  // position (squared) on the path at the last time step (used to detect finish)

    uint8_t num_segs;       // number of time segments being used
    struct {
        float jerk_ref;     // jerk reference value for time segment (the jerk at the beginning, middle or end depending upon the segment type)
        enum SegmentType seg_type;   // segment type (jerk is constant, increasing or decreasing)
        float end_time;     // final time value for segment
        float end_accel;    // final acceleration value for segment
        float end_vel;      // final velocity value for segment
        float end_pos;      // final position value for segment
    } segment[segments_max];

    Vector3f_t track;       // total change in position from origin to destination
    Vector3f_t delta_unit;  // reference direction vector for path
};

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void scurve_ctor(scurve_t scurve);
void scurve_init(scurve_t scurve);

// generate a trigonometric track in 3D space that moves over a straight line
// between two points defined by the origin and destination
void scurve_calculate_track(scurve_t scurve, const Vector3f_t *origin, const Vector3f_t *destination,
                             float speed_xy, float speed_up, float speed_down,
                             float accel_xy, float accel_z,
                             float snap_maximum, float jerk_maximum);
void scurve_calculate_path(float Sm, float Jm, float V0, float Am, float Vm, float L,
                             float *Jm_out, float *tj_out,  float *t2_out, float *t4_out, float *t6_out);

// set maximum velocity and re-calculate the path using these limits
void scurve_set_speed_max(scurve_t scurve, float speed_xy, float speed_up, float speed_down);

// set the maximum vehicle speed at the origin
// returns the expected speed at the origin which will always be equal or lower than speed
float scurve_set_origin_speed_max(scurve_t scurve, float speed);

// set the maximum vehicle speed at the destination
void scurve_set_destination_speed_max(scurve_t scurve, float speed);

// move target location along path from origin to destination
// prev_leg and next_leg are the paths before and after this path
// wp_radius is max distance from the waypoint at the apex of the turn
// fast_waypoint should be true if vehicle will not stop at end of this leg
// dt is the time increment the vehicle will move along the path
// target_pos should be set to this segment's origin and it will be updated to the current position target
// target_vel and target_accel are updated with new targets
// returns true if vehicle has passed the apex of the corner
bool scurve_advance_target_along_track(scurve_t scurve, scurve_t prev_leg, scurve_t next_leg, float wp_radius, float accel_corner, bool fast_waypoint, float dt, Vector3f_t *target_pos, Vector3f_t *target_vel, Vector3f_t *target_accel);

// time has reached the end of the sequence
bool scurve_finished(scurve_t scurve);
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



